
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       baro_ground_effect.c
  * @author     baiyang
  * @date       2022-8-29
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
#include <parameter/param.h>
#include <common/time/gp_time.h>
/*-----------------------------------macro------------------------------------*/
#define GYRO_VIBE_METRIC_MAX   0.02f // gyro_vibration_metric * dt * 4.0e4f > is_moving_scaler)
#define ACCEL_VIBE_METRIC_MAX  1.2f  // accel_vibration_metric * dt * 2.1e2f > is_moving_scaler
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void update_vehicle_at_rest();
/*----------------------------------variable----------------------------------*/
static uitc_vehicle_gndeffect gndeffect_detector = {
        .armed = false,
        .freefall = false,
        .ground_contact = true,
        .maybe_landed = true,
        .landed = true,
        .in_ground_effect = false,
        .at_rest = true};

static bool _at_rest = true;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]   pGndeffectState  
  * @param[out]  
  * @retval      
  * @note        
  */
static void publish_gndeffect_detector()
{
    DEFINE_TIMETAG(gndeffect_state_publish, 1000);

    bool in_ground_effect = fms.gndeffect_state.takeoff_expected || fms.gndeffect_state.touchdown_expected;

    const bool at_rest = fms.ap.land_complete && _at_rest;

    if (time_check_tag(TIMETAG(gndeffect_state_publish)) || 
        (gndeffect_detector.armed != fms.motors->_armed) || 
        (gndeffect_detector.landed != fms.ap.land_complete) || 
        (gndeffect_detector.maybe_landed  != fms.ap.land_complete_maybe) || 
        (gndeffect_detector.in_ground_effect != in_ground_effect) || 
        (gndeffect_detector.at_rest != at_rest)) {

        gndeffect_detector.armed = fms.motors->_armed;
        gndeffect_detector.landed = fms.ap.land_complete;
        gndeffect_detector.maybe_landed = fms.ap.land_complete_maybe;
        gndeffect_detector.in_ground_effect = in_ground_effect;
        gndeffect_detector.at_rest = at_rest;

        itc_publish(ITC_ID(vehicle_gndeffect), &gndeffect_detector);
    }
}

void fms_update_ground_effect_detector(void)
{
    if(!fms.g.gndeffect_comp_enabled || !fms.motors->_armed) {
        // disarmed - disable ground effect and return
        fms.gndeffect_state.takeoff_expected = false;
        fms.gndeffect_state.touchdown_expected = false;

        publish_gndeffect_detector();
        return;
    }

    // variable initialization
    uint32_t tnow_ms = time_millis();
    float xy_des_speed_cms = 0.0f;
    float xy_speed_cms = 0.0f;
    float des_climb_rate_cms = posctrl_get_vel_desired_cms(fms.pos_control).z;

    if (posctrl_is_active_xy(fms.pos_control)) {
        xy_des_speed_cms = vec3_length_xy(&fms.pos_control->_vel_target);
    }

    if (fms_position_ok() || fms_ekf_has_relative_position()) {
        xy_speed_cms = vec3_length_xy(&fms.ahrs->velocity_cm);
    }

    // takeoff logic

    if (mode_number(fms.flightmode) == THROW) {
        // throw mode never wants the takeoff expected EKF code
        fms.gndeffect_state.takeoff_expected = false;
    } else if (fms.motors->_armed && fms.ap.land_complete) {
        // if we are armed and haven't yet taken off then we expect an imminent takeoff
        fms.gndeffect_state.takeoff_expected = true;
    }

    // if we aren't taking off yet, reset the takeoff timer, altitude and complete flag
    const bool throttle_up = mode_has_manual_throttle(fms.flightmode) && fms.channel_throttle->control_in > 0;
    if (!throttle_up && fms.ap.land_complete) {
        fms.gndeffect_state.takeoff_time_ms = tnow_ms;
        fms.gndeffect_state.takeoff_alt_cm = ahrs_get_position_z_up_cm(fms.ahrs);
    }

    // if we are in takeoff_expected and we meet the conditions for having taken off
    // end the takeoff_expected state
    if (fms.gndeffect_state.takeoff_expected && (tnow_ms-fms.gndeffect_state.takeoff_time_ms > 5000 || ahrs_get_position_z_up_cm(fms.ahrs)-fms.gndeffect_state.takeoff_alt_cm > 50.0f)) {
        fms.gndeffect_state.takeoff_expected = false;
    }

    // landing logic
    Vector3f_t angle_target_rad = attctrl_get_att_target_euler_rad(fms.attitude_control);
    bool small_angle_request = cosf(angle_target_rad.x)*cosf(angle_target_rad.y) > cosf(radians(7.5f));
    bool xy_speed_low = (fms_position_ok() || fms_ekf_has_relative_position()) && xy_speed_cms <= 125.0f;
    bool xy_speed_demand_low = posctrl_is_active_xy(fms.pos_control) && xy_des_speed_cms <= 125.0f;
    bool slow_horizontal = xy_speed_demand_low || (xy_speed_low && !posctrl_is_active_xy(fms.pos_control)) || (mode_number(fms.flightmode) == ALT_HOLD && small_angle_request);

    bool descent_demanded = posctrl_is_active_z(fms.pos_control) && des_climb_rate_cms < 0.0f;
    bool slow_descent_demanded = descent_demanded && des_climb_rate_cms >= -100.0f;
    bool z_speed_low = fabsf(ahrs_get_velocity_z_up_cms(fms.ahrs)) <= 60.0f;
    bool slow_descent = (slow_descent_demanded || (z_speed_low && descent_demanded));

    fms.gndeffect_state.touchdown_expected = slow_horizontal && slow_descent;

    update_vehicle_at_rest();

    // Prepare the EKF for ground effect if either takeoff or touchdown is expected.
    publish_gndeffect_detector();
}

static void update_vehicle_at_rest()
{
    static uint64_t _time_last_move_detect_us = 0;

    if ((fms.ahrs->gyro_vibration_metric > GYRO_VIBE_METRIC_MAX)
            || (fms.ahrs->accel_vibration_metric > ACCEL_VIBE_METRIC_MAX)) {

            _time_last_move_detect_us = fms.ahrs->timestamp_us;
    }

    _at_rest = (time_micros64() - _time_last_move_detect_us > 1000000);
}
/*------------------------------------test------------------------------------*/


